#include "modules/drivers/lidar/orin_hesai/driver/lidar_hesai_driver.h"

namespace apollo {
namespace drivers {
namespace lidar_hesai {

bool LidarHesaiDriver::Init() {
  if (node_ == nullptr) {
    AERROR << "node is nullptr";
    return false;
  }

  packet_buffer_ =
      std::make_shared<CircularQueueBuffer<LidarPacket>>(PACKET_BUFFER_SIZE);
  pc_buffer_ =
      std::make_shared<CircularQueueBuffer<PointCloud>>(FRAME_BUFFER_SIZE);

  receiver_ = std::make_shared<LidarReceiver>();
  if (!receiver_->Init(packet_buffer_, config_.firing_data_port())) {
    AERROR << "Failed to init receiver";
    return false;
  }
  parser_ = std::make_shared<LidarParser>();
  if (!parser_->Init(packet_buffer_, pc_buffer_)) {
    AERROR << "Failed to init parser";
    return false;
  }
  publisher_ = std::make_shared<LidarPublisher>();
  if (!publisher_->Init(config_.pc_channel(),node_, pc_buffer_, config_.frame_id())) {
    AERROR << "Failed to init publisher";
    return false;
  }
  thread_receiver_ = std::thread(&LidarReceiver::Start, receiver_.get());
  thread_parser_ = std::thread(&LidarParser::Start, parser_.get());
  thread_publisher_ = std::thread(&LidarPublisher::Start, publisher_.get());

  return true;
}

LidarHesaiDriver::~LidarHesaiDriver() {
  if (thread_receiver_.joinable()) {
    thread_receiver_.join();
  }
  if (thread_parser_.joinable()) {
    thread_parser_.join();
  }
  if (thread_publisher_.joinable()) {
    thread_publisher_.join();
  }
}

}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo
